#!/usr/bin/env python
# coding=utf-8
#import os
#import cv2
#import numpy as np
#import rospy 
#from std_msgs.msg import Header
#from sensor_msgs.msg import Image,PointCloud2
#import sensor_msgs.point_cloud2 as pcl2
#from cv_bridge import CvBridge

from kitti_data_utils import *
from publish_utils import *

DATA_PATH ='/home/passoni/文档/kitti/RawData/2011_09_26/2011_09_26_drive_0005_sync/'

if __name__ =='__main__':
    frame = 0
    rospy .init_node('kitti_node')
    cam_pub = rospy.Publisher('kitti_cam',Image,queue_size=3)

    # plc_pub = rospy.Publisher('kitti_point_cloud',PointCloud2,queue_size=2)
    plc_pub = rospy.Publisher('/kitti/velo/pointcloud', PointCloud2, queue_size=10)

    ego_pub = rospy.Publisher('kitti_ego_car',MarkerArray,queue_size=10)
    imu_pub = rospy.Publisher('kitti_imu',Imu,queue_size=10)
    gps_pub = rospy.Publisher('kitti_gps',NavSatFix,queue_size=10)
    bridge = CvBridge()

    rate =rospy.Rate(10)
    while not rospy.is_shutdown():
	image =read_camera(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))  #opencv格式图像
	publish_camera(cam_pub,bridge,image)

	point_cloud = read_velodyne(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame))
	publish_point_cloud(plc_pub, point_cloud)
	
	publish_ego_car(ego_pub)
	
	imu_data = read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))
	publish_imu(imu_pub, imu_data)
	publish_gps(gps_pub,imu_data, log=True)

        rospy.loginfo('published')
        rate.sleep()
        frame += 1
        frame %=154
